﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;
using Emgu.CV;
using Emgu.CV.Structure;
using Emgu.CV.Features2D;
using Emgu.CV.Util;
using Emgu.Util;
using Emgu.CV.CvEnum;
using Emgu.CV.UI;


namespace NAO_Image_Processing
{
    class ImageProcessor
    {
        int circleRadius;
        int circleThickness;
        Size textPosA;
        Size textPosC;
        double textScale;
        double textThickness;


        public Image<Bgr, Byte> drawDistance(Image<Bgr, Byte> inputImageC, List<List<double>> values,int resolutionHeigth)
        {
            double bwVertical = 47.64; //Vertikaler Blickwinkel der Kamera
            //double pixelProGrad = (inputImageC.Height / bwVertical);
            List<double> distanceValuesC = values.ElementAt(0);
            List<double> angleValues = values.ElementAt(1);
            List<double> distanceValuesA = values.ElementAt(2);
            double pixelProCM = ((resolutionHeigth / 2)/(distanceValuesA[0] * 100 - distanceValuesA[distanceValuesA.Count - 1] * 100));
            List<Point> points = new List<Point>();
            for (int i = 0; i < distanceValuesA.Count; i++)
            {
                points.Add(new Point((int)(inputImageC.Width / 2), (int)((inputImageC.Height / 2) + ((distanceValuesA[0] * 100 - distanceValuesA[i] * 100) * pixelProCM))));
            }
            /*Punkte der Linie vertikalen Linie*/
            Point point_top = points[0];
            Point point_bottom = points[points.Count - 1];
            /*Punkte und Linie einzeichnen*/
            MCvFont font = new MCvFont();
            CvInvoke.cvInitFont(ref font,
                                Emgu.CV.CvEnum.FONT.CV_FONT_HERSHEY_COMPLEX,
                                textScale,
                                textScale,
                                textThickness,
                                0,
                                Emgu.CV.CvEnum.LINE_TYPE.CV_AA);
            for (int i = 0; i < points.Count; i++)
            {
                CvInvoke.cvCircle(inputImageC, //Bild für Operation
                                    points[i], //Kreismittelpunkt
                                    circleRadius, //Radius
                                    new MCvScalar(0, 255, 0), //Farbe (Weiss)
                                    circleThickness, //Dicke des Rands
                                    Emgu.CV.CvEnum.LINE_TYPE.CV_AA, //Linientyp 
                                    0 //Shift
                                    );
                CvInvoke.cvPutText(inputImageC, //Bild für Operation
                                    (int)(distanceValuesA[i] * 100) + " cm", //Ausgabetext
                                    Point.Add(new Point(points[i].X, points[i].Y),textPosA), //Position des Textes (Nach links oben versetzt)
                                    ref font, //Textart
                                    new MCvScalar(255, 255, 255) //Farbe (Weiss)
                                    );
                CvInvoke.cvPutText(inputImageC, //Bild für Operation
                                    (int)(distanceValuesC[i]*100) + " cm", //Ausgabetext
                                    Point.Add(new Point(points[i].X, points[i].Y), textPosC), //Position des Textes (Nach rechts oben versetzt)
                                    ref font, //Textart
                                    new MCvScalar(255, 255, 255) //Farbe (Weiss)
                                    );
                
            }
            CvInvoke.cvLine(inputImageC, point_top, point_bottom, new MCvScalar(255, 255, 255), 1, LINE_TYPE.CV_AA, 0);
            return inputImageC;
        }

        public Image<Bgr, Byte> SURF(Image<Bgr, Byte> inputImageC, Image<Bgr, Byte> outputImageC)
        {
            /*SURF*/
            Image<Gray, Byte> inputImageG = inputImageC.Convert<Gray, Byte>();
            SURFDetector surfCPU = new SURFDetector(500, false);
            VectorOfKeyPoint modelKeyPointsSURF = new VectorOfKeyPoint();
            Matrix<float> modelDescriptorsSURF = surfCPU.DetectAndCompute(inputImageG, null, modelKeyPointsSURF);
            /*Keypoints in das Ausgangsbild für SURF einzeichnen*/
            outputImageC = Features2DToolbox.DrawKeypoints(outputImageC, modelKeyPointsSURF, new Bgr(Color.Red), Features2DToolbox.KeypointDrawType.DEFAULT);

            return outputImageC;
        }

        public Image<Bgr, Byte> HoughLines(Image<Bgr, Byte> inputImageC, Image<Bgr, Byte> outputImageC)
        {
            LineSegment2D[][] lines = inputImageC.HoughLines(175, 550, 1.0, Math.PI / 180, 20, 20, 10);
            for (int i = 0; i < lines[0].Length; i++)
            {
                CvInvoke.cvLine(outputImageC.Ptr, lines[0][i].P1, lines[0][i].P2, new MCvScalar(0, 255, 0), 1, LINE_TYPE.CV_AA, 0);
            }

            return outputImageC;
        }

        public void setScreenParameters(int resolutionHeigth)
        {
            switch (resolutionHeigth)
            {
                case 120: 
                    {
                        circleRadius = 1;
                        circleThickness = 1;
                        textPosA = new Size(-32, -3);
                        textPosC = new Size(10, -3);
                        textScale = 0.18;
                        textThickness = 0;
                        break;
                    }
                case 240:
                    {
                        circleRadius = 2;
                        circleThickness = 1;
                        textPosA = new Size(-40, -3);
                        textPosC = new Size(10, -3);
                        textScale = 0.23;
                        textThickness = 0;
                        break;
                    }
                case 480:
                    {
                        circleRadius = 4;
                        circleThickness = 1;
                        textPosA = new Size(-75, -3);
                        textPosC = new Size(15, -3);
                        textScale = 0.5;
                        textThickness = 0;
                        break;
                    }
                case 960:
                    {
                        circleRadius = 6;
                        circleThickness = 1;
                        textPosA = new Size(-175, -3);
                        textPosC = new Size(40, -3);
                        textScale = 1.25;
                        textThickness = 2;
                        break;
                    }
                default: break;
            }
        }
    }
}
